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ABSTRACT 

A new robust, optimal, adaptive technique for compensating rate and position limits in the joints 
of a six degre e-o f- freedom elbow manipulator is presented. In this new algorithm, the unmet 
demand as a result of actuator saturation is redistributed among the remaining unsaturated joints. 
The scheme is used to compensate for inadequate path planning, problems such as joint limiting, 
joint freezing, or even obstacle avoidance, where a desired position and orientation are not 
attainable due to an unrealizable joint command. Once a joint encounters a limit, supplemental 
commands arc sent to other joints to best track, according to a selected criterion, the desired 
trajectory. 

INTRODUCTION 

A standard six degree-of-freedotn elbow manipulator (figure 1 ) has six independently controlled 
joints. The position and orientation of the end effector, each of which is described in three 
dimensions, are fully determined by the angles of the joints. As long as the appropriate joint 
angles are achievable, the desired position mid orientation can be obtained. However, when the 
specified joint trajectories cannot be followed due to a command beyond the range of the 
actuator, positions and orientations downstream from the limited joir* will all be affected, causing 
in some cases extreme deviations from the expected values. The Windup Feedback scheme [1] 
is aii ideal solution candidate for this problem. It was designed to compensate for actuator 
saturation in a multivariable system by supplementing the commands to the remaining actuators 
to produce the desired effect on the output, in this case the gripper position and orientation. For 
each joint which saturates, a degree of freedom is lost, but the remaining joints can be used to 
track the desired path witliin the physical limits of the manipulator. 
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MATHEMATICAL BACKGROUND FOR ROBOT JOINT CALCULATIONS 

An overview of the mathematical descriptions used for robot joint calculations will be presented 
in this section. For a more thorough presentation, the reader is referred to [2]. 

In order to describe the position and orientation of a robot’s end effector in space, we will define 
six Cartesian (x, y, 2 ) coordinate frames, one at each joint. The main reference frame is fixed 
such that the base of the robot is at the origin, as shown in figure 1. The five other reference 
frames are each attached to one of the other joints. Tnus the position and orientation of the end 
effector with respect to any joint is known. A transformation from one reference frame to 
another, consisting of rotations and translations, can be described by the 4x4 transformation 
matrix 

n 0 a p 

x * x J x 

n 0 a p 
n o a p 
0 0 0 1 

where i refers to the original coordinate frame and j refers to the transformed coordinate frame. 
The orthonormal 0-, and a-vectors describe the orientation as shown in figure i while the p- 
vector provides the position information. In a robo 1 manipulator, a transformation matrix *Tj+t 
can be defined to describe the rotation and translation required to get from the /th to the j+./st 
joint using the convention tliat the motion of the /tb. link is along the 2 -axis of the /th joint if it 
is trtutslational, and around the 2 -axis of the /th joint if it is rotational. Multiplying ihe matrices 
describing sequential joint transformations will give a new transformation matrix from the first 
joint in the series to the last. Thus, in a six -jointed manipulator, °T 6 is the transformation from 
the base to the gripper in base coordinates, i.e., represents the position and orientation of the 
end effector in base coordinates. 
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Finally, in order to see what effect a differential change in any joint (dq t ) ha3 on the gripper 
position artd orientation, a 6x6 matrix known as the Jacobian is defined. The Jacobian, .7, can 
be used, to compute differential changes in jiosition (</,) and orientation (6,) based on differential 
changes in joint translations and singles as 
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where the vector dq corresponds to differential joint movements, either translational or rotational, 
and D represents their corresponding effects at the gripper, The Jacobian is the first derivative 
of the equations of motion with respect to each joint. A first-order approximation of the Jacobian 
is easily obtained from the transformation matrices from each joint to the gripper »X) 

using the equations 


d M • »*((6 x p) + «0 
d y * o-((b*p)+d) 
d, - a-((b*p) + d) 
b % m K 'b 
6 o • b 

b a-b 

X 

where d and 6 indicate translational and rotational movement of the joint, respectively. They 
arc defined as d - (0,0,1), 5 *= (0,0,0) for prismatic joints and d - (0,0,0), 6 - (0,0,1) for 
rotational joints. Using these relationships, the Jacobian can be computed as 


J <* 
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where the subcrijrts from 0 through 5 use the values from die transformation matrices % through 
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MATHEMATICAL DEVELOPMENT OF THE WINDUP FEEDBACK SCHEME 
The Windup Feedback scheme is an algorithm developed to take advantage of underutilized 
actuators to comjxnsate for saturated actuators such that the output of the system optimally tracks 
the output of a similar system without actuator limits. 

In a robot manipulator, saturation can occur when a command to a joint is too large to be 
accommodated, either in position or rate, such as a request to rotate a joint to 110* when it is 
restricted to lie within the ±90* range, or a request to move 110* in one second when the rate 
limit is 90" pet second- In a situation where each joint single is computed anil commanded based 
on a desired position sand orientation, a joint which cannot track its command will prevent die 
gripper from reaching its desired position. By using odier joints to compensate for the saturated 
one, the desired gripper position can lx nearly matched and the robot manipulator might be able 
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to perform its task as if no joint reached its limit. Figure 2 depicts a robotic system with joint 
commands altered by Windup Feedback gains so that the position and orientation of the end 
effector track their ideal counterparts even during position and rate limits. In figure 2, q is the 
vector of ideal joint commands, and Aq is the vector of the difference between the desired joint 
commands and the achievable commands. When at least one joint is at its limit, q‘ is the vector 
of optimized supplemental commands to compensate the saturated joint commands. If Aq is 
relatively small, it approximates dq from (1). Using the definitions from the previous section, 
we can derive the Windup Feedback scheme as applied to manipulator systems. 

The Windup Feedback scheme tries to minimize the difference between the desired and 
achievable end effector position and orientation in an optimal sense. At every control interval, 
a command is given to each joint with the goal of moving the gripper along a desired trajectory. 
If a desired command is not achievable because it would force a joint to move beyond its limit, 
the Windup Feedback scheme will try to utilize other, unsaturated joints to maneuver the effector 
to the desired position and orientation at the current time step. Thus, the quadratic performance 
index, PI, for this optimization procedure is defined as 

pi - j{[/(A* - i’i' T q ')f Q[j(Lq - rrV)] ♦ q‘ T rr T nrr T q-) (2) 

As shown in figure 2. A q is the vector of unmet demand, i.e. the difference between the desired 
joint commands and the achievable commands when a joint is at its limit, 'rhus, JAq 
approximates the differential change in gripper position and orientation, D, from (1), required to 
move to the desired location based on the ideal commands. The vector q' consists of the 
optimized supplemental commands to comjxnsate the saturated joint commands as shown in 
figure 2. The diagonal weighting matrix Q allows more importance to be given to selected 
variables, such as position over orientation. The diagonal weighting matrix R penalizes the use 
of particular joints for compensation, and f is a matrix which restricts the supplemental joint 
commands t o be distributed over the unsaturated joints, f is created by taking the identity matrix 
of dimension equal to the number of joints and deleting each column which corresponds to a 
command greater than t^e joint’s limit, 'rhis way, whenever a limit is encountered. /* is 
computed to be the dimension of the total number of joints by the total number of unlimited 
joints. In the objective function (2) above, the formulation using two quadratic terms, 
corresponding to Q and R, provides a great advantage over the strict least squares formulation 
( Q only), as will be shown. 

The Windup Feedback gains are obtained by minimizing (2) with respect to q (see Appendix 
A for the derivation) to produce the solution 


q' - I'(l' T J T Qjr + r r RI m ) 'l’ r J r QJAq 


(3) 
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The elements of q* are the supplemental control commands which, when added to the commands 
to the unlimited joints, bring the end effector closer to the desired position and orientation. The 
unmet demand, L\q, can be represented as 


A* *■ 

/ 


where e { is a column vector of zeroes with a 1 in the rth location. The breaking up of the vector 
of unmet demand into its individual components allows each saturated joint to be compensated 
individually. Thus, if a single joint encounters its limit, a sing) A column of the Windup Feedback 
matrix can be computed using (3) with an /* matrix equal to the identity matrix with the 
appropriate column deleted. If, after tire addition of the supplemental q' terms, another joint 
saturates, the overdemand is again k Jis f ibuted amoi.^ the remaining unsaturated actuators 
through a second column of the Windup Feedback matrix determined using a new f equal to the 
previous ( with a second column deleted. This process can continue as long as at least one joint 
is not fully utilized. Thus, the ability to break up the A? vector into its components permits 
individual columns of the feedback matrix to be computed as needed. Using this technique, the 
computed columns correspond only to the saturated joints and allow redistribution only to the 
unsaturated joints, while the gains are continuously, optimally updated. This promotes the 
smooth flow of compensation between joint commands; because, immediately after a joint 
saturates, the overdemand to it is small so„ as it grows, the supplemental commands fed to the 
unsaturated joints are smooth, continuous signals. 

As stated earlier, the inclusion of the weighting matrix R in the objective function benefits the 
solution greatly. Even though the addition of the R term means that the solution obtained will 
not be strictly the best achievable match in a least squares sense to the desired solution, it forces 
the supplemental commands to stay close to their nominal values and thereby limits severe jumps 
and sign changes in the computed gains, effectively acting as a smoothing filter for the time- 
varying gains and resulting in a potentially much less erratic set of supplemental commands. 
Perhaps more importandy from an implementation standpoint, the inclusion of R guarantees the 
invertibility of the matrix, in (3). Using only the weighting matrix Q {R=QxI 6 ), die invert! bility 
of die matrix is not guaranteed as die manipulator moves through its workspace, even if Q is 
invertible. When joints are lined up along an axis, such as when the robot arm is (straight, the 
Jacobian, J, may become rank-deficient or at least have an unreliable numerical inverse. Using 
the above formulation, with the inclusion of die matrix R, the matrix to invert is in die Modified 
form [3], and in this special case it is nonsingular since R is invertible; it does not depend upon 
the rank of J. See Appendix B for a derivation of this result. 

EXAMPLES 

A six-jointed elbow manipulator, such as that shown in figure 1, ii; used in two examples to 
demonstrate the Windup Feedback Algorithm . The first illustrates rate Limit compensation, the 
second features position limit compensation. All joints' position and rate limits are displayed in 
Table I. 
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Table I. ELBOW MANIPULATOR MOTION LIMITS 


JOINT 

POSITION RANGE 

RATE LIMIT 

0, 

-90* through 90” 

90*/second 

e 2 

0* through 180* 

90*/second 

e s 

-90* through 90’ 

90*/second 

04 

-90’ through 90* 

90*/second 

05 

0* through 180* 

90*/secctnd 

06 

-90* through 90’ 

90 ’/second 


The path planning algorithm used here simply interpolates from starting point to ending point by 
incrementing each joint’s command by an amount related to the distance from the nearest 
endpoint. This gives a bill- shaped velocity profile (stopped at the beginning, fastest in the 
middle, stopped at the end). From figure 2 it is clear that the Windup Feedback algo; 'thm is 
applied to the joint commands only, not to the actual, measured joint angles. The purpose of this 
scheme is to provide admissible joint commands, i.e. commands -which the joints can physically 
follow which will result in the desired position and orientation. Therefore, the way the limit 
checking is incorporated is significant because that determines whether the joints will truly be 
able to track the commands. For these examples, the rate limit checking was implemented by 
determining the maximum angle the joint can rotate through in one time step b "ed on the 
maximum angular velocity listed in Table I, not taking acceleration into account, and allowing 
a command change of not more than that amount. The use of a more sophisticated rate limit 
checking computation utilizing acceleration limits and current velocity would not change the 
Windup Feedback algorithm in any way. The weighting matrix Q should be chosen depending 
on the task, but usually the position is compensated at the expense of the orientation, since most 
tasks will allow a larger error in approach than in position. The weighting matrix. R should be 
chosen such that it is a diagonal matrix with all elements positive. Beyond that, the Windup 
Feedback gains are relatively insensitive to large changes in R as long as it is of the form R~fcxJ 6 
with k>0. In cases where all diagonal elements of R are not the same, the potential exists to 
significantly alter the results by heavily penalizing the use of effective joints over ineffective ones 
for compensation. Unwise choices of R aside, its inclusion should have very little effect on the 
compensated position and orientation. The weighting matrices used in the following examples 
are 2=dwg<100,100,100,l,l,l) and R-=lQxJ 6 . The total movement in each example takes one 
second vrith the commands updated at a frequency of 50 Hz. 

In the first example, the objective is to move from the initial position and orientation to the final 
position and orientation which are specified as 
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Table n contains the joint angles corresponding to those endpoints. 

Table U. JOINT ANGLES FOR ENDPOINTS IN RATE LIMIT EXAMPLE 


JOINT 

INITIAL ANGLE 

FINAL ANGLE 

0, 

0’ 

0’ 

®2 

40’ 

0’ 

03 

70* 

10* 

04 

30* 

60* 

05 

90’ 

90* 

1 

0’ 

0* 


In the first example, a rate limit is encountered by the third joint about one third o*f the way 
through the nan. The unmet demand is redirected to cither, unsaturafed joints through the Windup 
Feedback gains computed using (3) and an /* matrix created by removing the third column from 
a 6x6 identity matrix. After several control intervals, the supplemental command added to the 
already r apidly changing command to the second joint causes it to rate limit also. Thus, a second 
column of the Windup Feedback matrix is computed us ' g (3) but a new /*: a 6x6 identity matrix 
with both tie second and third columns removed. The second joint comes off its limit about two 
thirds of the way through the run, as the r ate of change of the commands decreases, leaving only 
the third joint saturated. This joint also comes off its limit near the end of the run, again aligning 
the compensated and ideal trajectory commands. Figure 3 shows in three dimensions the paths 
of the three cases: desired, limited without compensation, and limited with Windup Feedback. 
The projections show that the error is limited to the x-z-planc. This view depicts the trajectories 
through space without any reference to time. Thus a different example could have teen 
concocted where the saturated curve is perfectly overlaid on the ideal curve. For this reason, 
figure 4 displays the three curves with respect to time, clearly demonstrating how the saturated 
case lags behind the other two as die rate-limited joint is unable to track the demand. Figure 5 
depicts the supplemental command vector, q' t used to compensate the saturated command. 
Figure 6 contains plots of the joint commands for the three cases. In the 0, trace, the rate-limited 
command cannot track the ideal command, resulting in the immediate divergence of the other, 
compensated joint commands from the ideal case to maintain the end effector in its desired 
trajectory. The otter uncompensated commands track the ideal commands exactly. The 
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compensation is accomplished essentially with the second and forth joints, but when the second 
joint command also hits its rate limit, the other joints temporarily play a more prominent role. 
Figure 7 compares the error in gripper position of the saturated and compensated cases. The 
compensated case is significantly better than the saturated case which is not surprising since the 
supplemental commands were optimized to maintain position. Figure 8 compares the error in 
approach (the direction of the vector a from figure 1 , corresponding to the direction in which the 
gripper is pointing) for the two cases. Since orientation was not heavily weighted in this 
example, the fact that the compensated case is much better is not significant, but it shows that 
orientation is not markedly sacrificed to maintain position. 

In the second example, the objective is to move from the initial position and orientation to the 
final position and orientation which are specified as 
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Table El contains the joint angles corresponding to those endpoints. 


Table HI. JOINT ANGLES FOR ENDPOI NTS IN POSITION LIMIT EXAMPLE 


JOINT 

INITIAL ANGLE 

FINAL ANGLE 

e, 

0* 

0* 


45* 

45* 

e. 

-75* 

-110* 

A* 

0* 

0* 

e. 

45* """ 1 

45* 

e« 

0* 

0* 


Since the final desired value for the third joint is unrealizable, every succeeding joint, even if it 
has achieved its commanded angle, will not be at its desired position and orientation. In tills 
example, die third joint encounters its position limit at nearly half way through its desired swing. 
The unachievable command is redistributed! through the Windup Feedback gains to other joints. 
In doing so, a large enough supplement is added to the fourth joint: that it rides rate limit for 
several control intervals while the ideal command is changing at its fastest rate. This causes a 
second column of the Windup Feedback matrix to be computed, redistributing this unmet 
command smong the other four joints. Once the rare of command change has decreased enough. 
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the fourth joint comes off its limit while continuing to accommodate the unmet command to the 
third joint. Figure 9 shows the three-dimensional path the end effector follows. The 
compensated path lies nearly along the desired trajectory while the uncompensated path comes 
to a dead stop after saturation and never gets near its final destination. Figure 10 shows the x-> 
y-, and 2 -positions of the gripper versus time for the three paths. The compensated path tracks 
the desired closely in both x and z while paying a small penalty in y as compared to the 
uncompensated path which diverges from the other two in both x and z after saturation. Figure 
1 1. depicts the supplemental commands used to compensate the saturated joint commands. Figure 
12 displays ideal, compensated, and satu d commands with respect to time. The 
uncompensated curves exactly follow the ideas commands, except for the saturated 0 } curve, 
which is the only one that shows on the trace. The compensated 0, command’s constant, steep 
slope reveals that it is rate limited for a short time initially. Figure 13 compares the error in 
gripper position of the compensated and saturated cases. A great improvement is achieved 
through the use of Windup Feedback as the error is reduced to about 5% of that in the 
uncompensated case even though a position limit was encountered. Figure 14 compares the error 
in Approach of the two cases. Here again, orientation, in the compensated case is not significantly 
sacrificed to maintain position and is, in fact, better than in the uncompensated case. 

CONCLUSIONS 

The Windup Feedback scheme is a robust, optimal adaptive algorithm which has lieen shown to 
significantly improve the tracking of the desired end effector trajectory for a six -degree-of- 
fice dom elbow manipulator under unexpected rate and position constraints. The scheme is 
especially suitable for applications which include some variability so that unusual situations, such 
as joint saturations, are likely to occur. The weighting matrix Q should be chosen depending 
upon the task, to appropriately emphasize position or orientation. The inclusion of the weighting 
matrix JR gives a solution which is not the test fit, in a least squares sense, to the desired. 
However, the resulting difference in position and orientation between the optimal solution 
obtained using R and the feast squares solution should be negligible and the compensation 
variables should vary more smoothly than when R is not included. The Windup Feetlback gains 
are simple to compute and adapt online in rem time which makes this scheme practical. 

APPENDIX A: DERIVATION OF WINDUP FEEDBACK GAINS 
The objective function is defined as 


pi = --{[./( A q - rr r q -)fQ{j( a? - rrV)] + q' T rr r Rrr T q’} 

2 

with variables as show in figure 2. I" is created by taking the identity matrix of dimension equal 
to the number of joints and deleting each column which corresponds t o a command greater than 
the joint’s limit. This way, whenever a limit is encountered, /* is computed to be the dimension 
of the total number of joints by the total number of unlimited joints. Therefore, /* has more rows 
than columns and each column has exactly one 1. in it. It is clear that l‘l' J is a diagonal matrix 
of zeroes and ones and l‘ r f is the identity matrix. 
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PI is easily minimized as follows. 


— - = -{jbq - jrj' l q') T Qjrr l + q‘ T rr T nrr T = o 

dq' 

- -rr T j T Q(j&q - jrr T q") + n ,T Rrr T q ' = o 


Therefore, 


rr T j f QjA q 

r T rr T j T QjAq 


I‘ T J T QJAq 


(. rr T J T Qjrr T 
C r T rr T j T Qjrr T 
c r T j T Qjrr T 

(i r T j T Qjr 


+ rr T Rrr T )q m 
* r T rr T Rrr T )q" 
+ r T Rrr T ) q ’ 

+ r T Ri‘)r r q * 


As long as J‘ r J T QJI‘ 4 I' T RI * is full rank, it can be inverted, thus 


( r T J T Qjr * r T Rry'r r j r QjA q - /°v 

which, using the identity property of /*, leads to 


* * = r(i' T j r Qjr + i’ T Rry'i' T j T QjA q 

APPENDIX B: PROOF OF INVERTIBIUTY 

By definition, a matrix *4 is said to be positive sentidefinite (p.s.d.) if and only if , , . any 

vector x. In the ease where equality holds only when x is uniquely the zero vector, .aid to 
be positive definite (p.d.) [4]. The eigenvalues of a positive serradefinite aiairix are all 
nonnegative. The. eigenvalues of a positive definite matrix are all positive. Consequently, p.d. 
matrices are also p.s.d. but they are always invertible since all of their eigenvalues are nonzero. 

The inclusion of the diagonal weighting matrix R in die objective function (2) changes the matrix 
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to be inverted in (3) from being positive semidefinite to being positive definite and thus always 
invertible. This is easily shown as follows. 

The matrix to be inverted is: 


r'j'Qjr * r T nr (4) 

Hie weighting matrix Q is diagonal positive semidefinite (it may have some diagonal terms equal 
to zero) and R is diagonal positive definite. Note that a matrix A is p.s.d. if there exists a matrix 
T such that A*7*T [5]. Clearly both terms of (4) meet this condition, therefore they are both 
p.s.d. Additionally, the second term is p.d. because, independent of the number of columns of 
/*, it is a diagonal matrix with all elements greater than zero since they are merely selected 
diagonal elements of the original R matrix. Pm- and postmultiplying (4) by an arbitrary nonzero 
vector x gives 


x T {r T j T Qjr ♦ j ,t ri’)x x T r T j r QJi'x + x T r r RVx 


which, by the definition of a p.s.d. matrix, produces a scalar greater than or equal to zero for the 
first term plus a scalar greater titan zero for the second term. Thus, the sum is greater than zero 
for any nonzero vector x. Therefore, the matrix is p.d. and consequently always invertible. 
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Figure 6. Weal, compensated, and saturated commands in degrees vs. sample number for rate-timited example. 
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Figure 7 . Error' in gripper position vs. sample number for rate-Hmited example. 
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Figure 8. Angle error in approach vs. sample number for rate-limited example. 
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Figure 9. Three-dimensional view with projections, for the position-limited example. 
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Figure 1 1 . Supplemental commands In degrees vs. sample number for position-limited example. 
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sample number 

Figure 13. Error in gripper position vs. sample number for position-limited example. 
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Figure 14. Angle error In approach vs. sample number for position-limited example. 
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